/*! @file flight-control/main.cpp
 *  @version 4.0.0
 *  @date Jun 05 2017
 *
 *  @brief
 *  main for Flight Control API usage in a Linux environment.
 *  Provides a number of helpful additions to core API calls,
 *  especially for position control, attitude control, takeoff,
 *  landing.
 *
 *  @Copyright (c) 2016-2017 DJI
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 *
 */

/*TODO:flight_control_sample will by replace by flight_sample in the future*/
#include "flight_control_sample.hpp"
#include "flight_sample.hpp"
#include "dji_linux_helpers.hpp"
//lry
#include <netinet/in.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <json/json.h>
#include <string>

#define BUFFER_SIZE 1024

using namespace DJI::OSDK;
using namespace DJI::OSDK::Telemetry;

int main(int argc, char** argv) {
  //test network lry
  struct sockaddr_in server_addr;
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(7088);
    server_addr.sin_addr.s_addr = inet_addr("118.24.8.33");
    bzero(&(server_addr.sin_zero), 8);

    int server_sock_fd = socket(AF_INET, SOCK_STREAM, 0);
    if (server_sock_fd == -1)
    {
        perror("socket error");
        return 1;
    }
    char recv_msg[BUFFER_SIZE];
    char input_msg[BUFFER_SIZE];

    if (connect(server_sock_fd, (struct sockaddr *)&server_addr, sizeof(struct sockaddr_in)) == 0)
    {
        fd_set client_fd_set;
        struct timeval tv;

        while (1)
        {
            tv.tv_sec = 20;
            tv.tv_usec = 0;
            FD_ZERO(&client_fd_set);
            FD_SET(STDIN_FILENO, &client_fd_set);   // 标准输入  加入文件描述符集，让select监听输入事件 。
            FD_SET(server_sock_fd, &client_fd_set); // 服务端

            select(server_sock_fd + 1, &client_fd_set, NULL, NULL, &tv);
            if (FD_ISSET(STDIN_FILENO, &client_fd_set)) // 输入数据则发送
            {
                bzero(input_msg, BUFFER_SIZE);
                fgets(input_msg, BUFFER_SIZE, stdin);
                Json::Value data;
                Json::StyledWriter sWriter;
                data["type"] = "uva";
                data["x"] = 0.1;
                data["y"] = 0.2;
                data["z"] = 0.3;
                std::string data_str = sWriter.write(data);
                if (send(server_sock_fd, data_str.c_str(), BUFFER_SIZE, 0) == -1)
                {
                    perror("发送消息出错!\n");
                }
            }
            if (FD_ISSET(server_sock_fd, &client_fd_set))
            {
                bzero(recv_msg, BUFFER_SIZE);
                long byte_num = recv(server_sock_fd, recv_msg, BUFFER_SIZE, 0);
                if (byte_num > 0)
                {
                    if (byte_num > BUFFER_SIZE)
                    {
                        byte_num = BUFFER_SIZE;
                    }
                    recv_msg[byte_num] = '\0';
                    printf("服务器:%s\n", recv_msg);
                }
                else if (byte_num < 0)
                {
                    printf("接受消息出错!\n");
                }
                else
                {
                    printf("服务器端退出!\n");
                    exit(0);
                }
            }
        }
        //}
    }// end network / json test




  // Initialize variables
  int functionTimeout = 1;

  // Setup OSDK.
  LinuxSetup linuxEnvironment(argc, argv);
  Vehicle* vehicle = linuxEnvironment.getVehicle();
  if (vehicle == NULL) {
    std::cout << "Vehicle not initialized, exiting.\n";
    return -1;
  }

  // Obtain Control Authority
  // TODO: move this to flight controlller
  vehicle->control->obtainCtrlAuthority(functionTimeout);
  FlightSample* flightSample = new FlightSample(vehicle);
  // Display interactive prompt
  std::cout
      << "| Available commands:                                            |"
      << std::endl;
  std::cout
      << "| [a] Monitored Takeoff + Landing                                |"
      << std::endl;
  std::cout
      << "| [b] Monitored Takeoff + Position Control + Landing             |"
      << std::endl;
  std::cout << "| [c] Monitored Takeoff + Position Control + Force Landing "
               "Avoid Ground  |"
            << std::endl;

  char inputChar;
  std::cin >> inputChar;

  switch (inputChar) {
    case 'a':
     //sleep(60);
      monitoredTakeoff(vehicle);
      monitoredLanding(vehicle);
      break;
    case 'b':
	//sleep(60);
      monitoredTakeoff(vehicle);
      DSTATUS("Take off over!\n");
      moveByPositionOffset(vehicle, 0, 0, 3, 30);
      DSTATUS("Step 1 over!\n");
      //moveByPositionOffset(vehicle, 6, 0, -3, -30);
      DSTATUS("Step 2 over!\n");
      //moveByPositionOffset(vehicle, -6, -6, 0, 0);
      DSTATUS("Step 3 over!\n");
      monitoredLanding(vehicle);
      break;

    /*! @NOTE: case 'c' only support for m210 V2*/
    case 'c':
	//sleep(60);
      /*!  Take off */

      flightSample->monitoredTakeoff();
      vehicle->flightController->setCollisionAvoidanceEnabledSync(
          FlightController::AvoidEnable::AVOID_ENABLE, 1);

      /*! Move to higher altitude */
      flightSample->moveByPositionOffset((FlightSample::Vector3f){0.002, 0, 10}, 0);

      /*! Move a short distance*/
      flightSample->moveByPositionOffset((FlightSample::Vector3f){7, 0, 0}, 0);

      /*! Set aircraft current position as new home location */
      //flightSample->setNewHomeLocation();

      /*! Set new go home altitude */
      flightSample->setGoHomeAltitude(5);

      /*! Move to another position */
      flightSample->moveByPositionOffset((FlightSample::Vector3f){0, 7, 0}, 0);

      vehicle->flightController->setCollisionAvoidanceEnabledSync(
        FlightController::AvoidEnable::AVOID_DISABLE, 1);
      /*! go home and confirm landing */
      flightSample->goHomeAndConfirmLanding();

      vehicle->flightController->setCollisionAvoidanceEnabledSync(
        FlightController::AvoidEnable::AVOID_ENABLE, 1);
      break;
    default:
      break;
  }

  delete flightSample;
  return 0;
}
